/*
 * Pdsp.h
 *
 *  Created on: Oct 13, 2018
 *      Author: ax
 */
#include <string.h>

#include <Hw/Pdsp.h>

#include <ti/csl/cslr_pdsp.h>
#include <ti/csl/cslr_device.h>
#include <ti/csl/cslr_qm_intd.h>


volatile Uint32* const hQmssPdsp[2]= {
    (volatile Uint32*) CSL_QM_SS_CFG_SCRACH_RAM1_REGS,
    (volatile Uint32*) CSL_QM_SS_CFG_SCRACH_RAM2_REGS
};
//volatile Uint32* const hPassPdsp[2]= {
//    (volatile Uint32*)CSL_QM_SS_CFG_SCRACH_RAM1_REGS,
//    (volatile Uint32*)CSL_QM_SS_CFG_SCRACH_RAM2_REGS
//};

#define hQmssIntd ((CSL_Qm_intdRegs*) CSL_QM_SS_CFG_INTD_REGS)

Uint16
Pdsp_accumulatorSetup(
    Pdsp        pdsp,
    PdspAccCfg  cfg
){
    Uint32            cmd[5] = {0};
    volatile Uint32   *cmdPtr, *reg;
    Uint32            index;
    Uint8             result;

    CSL_FINSR (cmd[0], 7, 0, cfg->channel);
    CSL_FINSR (cmd[0], 15, 8, cfg->command);
    cmd[1] = cfg->queueEnMask;
    cmd[2] = cfg->listAddress;
    CSL_FINSR (cmd[3], 15, 0, cfg->queMgrIndex);
    CSL_FINSR (cmd[3], 31, 16, cfg->maxPageEntries);
    CSL_FINSR (cmd[4], 15, 0, cfg->timerLoadCount);
    CSL_FINSR (cmd[4], 17, 16, cfg->interruptPacingMode);
    CSL_FINSR (cmd[4], 19, 18, cfg->listEntrySize);
    CSL_FINSR (cmd[4], 20, 20, cfg->listCountMode);
    CSL_FINSR (cmd[4], 21, 21, cfg->multiQueueMode);

    /* Point to the accumulator command register's last word */
    reg = (Uint32 *) ((Uint8 *) hQmssPdsp[pdsp] + 4 * 4);

    /* Write command word last */
    cmdPtr = ((Uint32 *) &cmd) + 4;

    for (index = 0; index < 5; index++)
        *reg-- = *cmdPtr--;

    /* wait for the command to clear */
    reg++;
    do {
        result = CSL_FEXTR (*reg, 15, 8);
    } while (result != 0);

    /*clear interrupts*/
    hQmssIntd->INTCNT_REG[cfg->channel]= 0 ;
    hQmssIntd->EOI_REG= cfg->channel + 2;

    return (Uint32) (CSL_FEXTR (*reg, 31, 24));
}

void
Pdsp_firmwareLoad(
    Pdsp pid,
    const void* fw,
    Uint32 count
){
    CSL_PdspRegs* const pdsp[2]= {
        (CSL_PdspRegs*) CSL_QM_SS_CFG_ADSP1_REGS,
        (CSL_PdspRegs*) CSL_QM_SS_CFG_ADSP2_REGS };

    /* Reset the PDSP */
    CSL_FINS(pdsp[pid]->PDSP_CONTROL_REG, PDSP_PDSP_CONTROL_REG_PDSP_ENABLE, 0);
    /* Confirm PDSP has halted */
    while (CSL_FEXT (pdsp[pid]->PDSP_CONTROL_REG, PDSP_PDSP_CONTROL_REG_PDSP_STATE));


    volatile Uint32* PdspIRAM[2] = {
        (volatile Uint32*) CSL_QM_SS_CFG_APDSP1_RAM_REGS,
        (volatile Uint32*) CSL_QM_SS_CFG_APDSP2_RAM_REGS
    };
    /* Some firmwares need the 1 based PDSP number written to scratch + 0x18 */
    *(PdspIRAM[pid] + 6) = pid + 1;
    /*count >>= 2; Divided by 4 */
    /* Download the firmware
    Uint32 i;
    for(i = 0; i < count; i++)
        PdspIRAM[pid][i] = ((Uint32*) fw)[i]; */
    memcpy((void*) PdspIRAM[pid], fw, count);

    /* Use the command register to sync the PDSP */
    *hQmssPdsp[pid] = 0xFFFFFFFF;
    /* Wait to the memory write to land */
    for (Uint32 i = 0; (i < 20000) && (*hQmssPdsp[pid] != 0xFFFFFFFF); i++);

    /* Reset program counter to zero */
    CSL_FINS(pdsp[pid]->PDSP_CONTROL_REG, PDSP_PDSP_CONTROL_REG_PCOUNTER_RST_VAL, 0);
    /* Set soft reset to zero to load the program counter */
    CSL_FINS(pdsp[pid]->PDSP_CONTROL_REG, PDSP_PDSP_CONTROL_REG_SOFT_RST_N, 0);
    /* Enable the PDSP */
    CSL_FINS(pdsp[pid]->PDSP_CONTROL_REG, PDSP_PDSP_CONTROL_REG_PDSP_ENABLE, 1);

    /* Wait for the command register to clear */
    while (*hQmssPdsp[pid]);
}

Uint32
Pdsp_reclaimQueueSetup(
    Pdsp  pid,
    Queue que
){
    volatile Uint32   *reg;
    Uint8             result;

    /* Point to the accumulator command register's last word */
    reg = (Uint32*) ((Uint8*) hQmssPdsp[pid] + 4);
    *reg-- = que;
    *reg = Pdsp_AccCmd_CONFIG_RECLAIM_QUEUE << 8;

    /* wait for the command to clear */
    do {
        result = CSL_FEXTR (*reg, 15, 8);
    } while (result != 0);

    return (Uint32) (CSL_FEXTR (*reg, 31, 24));
}
